{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Control Reachy's gaze in Unity using the look_at function\n",
    "\n",
    "This notebook shows you how to control Reachy's head orientation by using the look_at function.\n",
    "\n",
    "The look_at function takes a few argument:\n",
    "* (x, y, z) the 3D coordinates of the point you want to look at (in Reachy's coordinate system : x is forward, y is left and z s up)\n",
    "* duration: head motion duration to go from its current orientation to the requested one (in sec.)\n",
    "* wait (bool): whether or not to wait for the end of the movement\n",
    "\n",
    "Below, we illustrate how to use it inside a loop to let you define new look target using slider widgets."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "from reachy import parts, Reachy\n",
    "\n",
    "reachy = Reachy(\n",
    "    left_arm=parts.LeftArm(io='ws', hand='force_gripper'),\n",
    "    right_arm=parts.RightArm(io='ws', hand='force_gripper'),\n",
    "    head=parts.Head(io='ws'),    \n",
    ")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You can now connect your robot in Unity."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "from IPython.display import clear_output\n",
    "\n",
    "\n",
    "class LookAt(object):\n",
    "    def __init__(self, head, init_target=(0.5, 0, 0), period=0.05):\n",
    "        self.head = head\n",
    "        \n",
    "        self.trajs = None\n",
    "        \n",
    "        self.target = init_target\n",
    "        self.period = period\n",
    "        \n",
    "    def set_target(self, x, y, z):\n",
    "        if self.target == (x, y, z):\n",
    "            return\n",
    "        \n",
    "        if self.trajs is not None:\n",
    "            for t in self.trajs:\n",
    "                t.stop()\n",
    "                \n",
    "        self.target = (x, y, z)\n",
    "        try:\n",
    "            self.trajs = self.head.look_at(x, y, z, duration=self.period, wait=False)\n",
    "        except (ValueError, ZeroDivisionError):\n",
    "            clear_output(wait=True)\n",
    "            print(f'target {self.target} out of reach!')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "look_at = LookAt(reachy.head)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "from ipywidgets import interact\n",
    "\n",
    "def loop(x, y, z):\n",
    "    look_at.set_target(x, y, z)\n",
    "    \n",
    "_ = interact(\n",
    "    loop,\n",
    "    x=0.5,\n",
    "    y=(-1, 1, 0.01),\n",
    "    z=(-1, 1, 0.01),\n",
    ")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.8.4"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}
